#include <ros/ros.h>
#include <iostream>
#include <fstream>
#include <Eigen/Eigen>
#include <serial/serial.h>
#include "printf_utils.h"
#include "prometheus_msgs/DetectionInfo.h"
#include "prometheus_msgs/UAVState.h"
#include "prometheus_msgs/UAVCommand.h"
#include "prometheus_msgs/UAVControlState.h"
#include "prometheus_msgs/UAVSetup.h"

class MySerial
{
public:
    MySerial(int baudrate, std::string port_name)
    {
        this->baudrate = baudrate;
        this->port_name = port_name; 
    }

    bool open_serial()
    {
        try
        {
            ser.setBaudrate(baudrate);
            ser.setPort(port_name);
            serial::Timeout to = serial::Timeout::simpleTimeout(1000); 
            ser.setTimeout(to); 
            ser.open(); 
        }
        catch(serial::IOException&  e)
        {
            std::cerr << e.what() << '\n';
            ROS_ERROR_STREAM("Unable to open port" << port_name); 
            return false;
        }
        
        if(ser.isOpen()) 
        { 
            ROS_INFO_STREAM("Serial Port initialized"); 
            return true;
        } 
        else 
        { 
            return false; 
        } 
        
    }

    void write_pos(float pos_x, float pos_y, u_int8_t type)
    {
        if(!this->ser.isOpen())
        {
            ROS_WARN_STREAM_THROTTLE(5, "Serial not opened, can not write data!");
            return;
        }
        data[0] = 0xFF;
        data[1] = type;
        data[2] = int(fabs(pos_x) * 50) > 255 ? 255: int(fabs(pos_x) * 50);
        data[3] = int(fabs(pos_y) * 50) > 255 ? 255: int(fabs(pos_y) * 50);
        data[4] = 0xFE;
        ser.write(data, 5);
    }

private:
    serial::Serial ser;
    u_int8_t data[5];
    std::string port_name;
    int baudrate;
};

using namespace std;

#define NODE_NAME "DS_G"
#define VISION_THRES 10

typedef Eigen::Vector2f Point;

struct Detection_result
{
    string object_name;
    // 视觉检测原始信息，返回的结果为相机坐标系
    // 方向定义： 识别算法发布的目标位置位于相机坐标系（从相机往前看，物体在相机右方x为正，下方y为正，前方z为正）
    // 标志位：   detected 用作标志位 ture代表识别到目标 false代表丢失目标
    prometheus_msgs::DetectionInfo Detection_info;      
    // 目标在相机系位置
    Eigen::Vector3f pos_camera_frame;  
    // 目标在机体系位置
    Eigen::Vector3f pos_body_frame;   
    // 目标在机体-惯性系位置 (原点位于质心，x轴指向前方，y轴指向左，z轴指向上的坐标系)
    Eigen::Vector3f pos_body_enu_frame;  
    // 目标在惯性系位置 (原点位于起飞点，x轴指向前方，y轴指向左，z轴指向上的坐标系)
    Eigen::Vector3f pos_enu_frame; 
    // 目标在机体系姿态
    Eigen::Vector3f att_body_frame;
    // 目标在惯性系姿态
    Eigen::Vector3f att_enu_frame;
    // 目标识别标志位,阈值:VISION_THRES
    bool is_detected = false; 
    int num_lost = 0;          //视觉丢失计数器
    int num_regain = 0;
};

Detection_result firepoint_det;

enum EXEC_STATE
{
    WaitingForStart,
    TakeOff,
    MovingAround,
    Tracking,
    Lost,
    AtFirePoint,
    ExecTask,
    Return,
    Landing
};

EXEC_STATE exec_state;

prometheus_msgs::UAVState uav_state;
prometheus_msgs::UAVControlState uav_control_state;
prometheus_msgs::UAVCommand uav_command;

void fire_point_cb(const prometheus_msgs::DetectionInfo::ConstPtr &msg)
{
    firepoint_det.object_name = "fire_point";
    firepoint_det.Detection_info = *msg;
    // 识别算法发布的目标位置位于相机坐标系（从相机往前看，物体在相机右方x为正，下方y为正，前方z为正）
    // 相机安装误差 在mission_utils.h中设置场
    // x, y轴交换
    firepoint_det.pos_body_frame[0] = -firepoint_det.Detection_info.position[1];
    firepoint_det.pos_body_frame[1] = -firepoint_det.Detection_info.position[0];
    firepoint_det.pos_body_frame[2] = -firepoint_det.Detection_info.position[2];

    // 机体系 -> 机体惯性系 (原点在机体的惯性系) (对无人机姿态进行解耦)
    // firepoint_det.pos_body_enu_frame = g_R_Body_to_ENU * firepoint_det.pos_body_frame;
    // printf("g_R_Body_to_ENU Matrix: %f %f %f",g_R_Body_to_ENU[0], g_R_Body_to_ENU[1], g_R_Body_to_ENU[2]);

    // 机体惯性系 -> 惯性系
    firepoint_det.pos_enu_frame[0] = uav_state.position[0] + firepoint_det.pos_body_enu_frame[0];
    firepoint_det.pos_enu_frame[1] = uav_state.position[1] + firepoint_det.pos_body_enu_frame[1];
    firepoint_det.pos_enu_frame[2] = uav_state.position[2] + firepoint_det.pos_body_enu_frame[2];
    // 此降落方案不考虑偏航角
    firepoint_det.att_enu_frame[2] = 0.0;

    if (firepoint_det.Detection_info.detected)
    {
        firepoint_det.num_regain++;
        firepoint_det.num_lost = 0;
    }
    else
    {
        firepoint_det.num_regain = 0;
        firepoint_det.num_lost++;
    }

    // 当连续一段时间无法检测到目标时，认定目标丢失
    if (firepoint_det.num_lost > VISION_THRES)
    {
        firepoint_det.is_detected = false;
    }

    // 当连续一段时间检测到目标时，认定目标得到
    if (firepoint_det.num_regain > VISION_THRES)
    {
        firepoint_det.is_detected = true;
    }
}

/******************        需要读取的参数！！     ***************************/
int uav_id;
string points_txt_path;

// 飞行高度、距离期望起飞高度的差值、moving-around中XY的差值、降落点XY差值
float target_height, takeoff_tolerance, moving_around_tolerance, landing_tolerance;
float min_vel_moving_around, max_vel_moving_around, min_vel_tracking, max_vel_tracking;

string portName;
int baudrate;

// 1m 高度下，摄像头看到的宽度，以及高度
float cam_view_width, cam_view_height, tracking_tolerance;

/************************************************************************/

void read_param(ros::NodeHandle &nh)
{
    nh.param<int>("uav_id", uav_id, 1);

    nh.param<string>("points_txt_path", points_txt_path, "/home/xs/catkin_ws/src/demo1/config/flying_points.txt");

    nh.param<float>("target_height", target_height, 1.5);

    nh.param<float>("takeoff_tolerance", takeoff_tolerance, 0.2);
    nh.param<float>("moving_around_tolerance", moving_around_tolerance, 0.2);
    nh.param<float>("landing_tolerance", landing_tolerance, 0.2);

    nh.param<float>("min_vel_moving_around", min_vel_moving_around, 0.1);
    nh.param<float>("max_vel_moving_around", max_vel_moving_around, 0.5);
    nh.param<float>("min_vel_tracking", min_vel_tracking, 0.05);
    nh.param<float>("max_vel_tracking", max_vel_tracking, 0.3);

    // 串口配置
    nh.param<int>("baudrate", baudrate, 115200);
    nh.param<string>("portName", portName, "/dev/stm32");

    // 追踪时参数
    nh.param<float>("cam_view_width", cam_view_width, 3.0);
    nh.param<float>("cam_view_height", cam_view_height, 1.8);
    nh.param<float>("tracking_tolerance", tracking_tolerance, 0.1);
}

void print_param(vector<Point> &moving_points)
{
    //固定的浮点显示
    cout.setf(ios::fixed);
    // setprecision(n) 设显示小数精度为n位
    cout << setprecision(NUM_POINT);
    //左对齐
    cout.setf(ios::left);
    // 强制显示小数点
    cout.setf(ios::showpoint);
    // 强制显示符号
    cout.setf(ios::showpos);

    cout << GREEN << ">>>>>>>>>>>>>>>    Dian Sai 2023 G question     <<<<<<<<<<<<<<<" << TAIL << endl;
    cout << GREEN << "uav id                           : " << uav_id << TAIL << endl; 
    cout << GREEN << "min/max vel when moving around   : " << min_vel_moving_around << " [m/s] to " << max_vel_moving_around << " [m/s]" << TAIL << endl; 
    cout << GREEN << "min/max vel when tracking        : " << min_vel_tracking << " [m/s] to " << max_vel_tracking << " [m/s]" << TAIL << endl; 

    cout << GREEN << "target fly height                : " << target_height << " [m]" << TAIL << endl; 
    cout << GREEN << "flying_points_file_path          : " << points_txt_path << TAIL << endl;

    cout << GREEN << "takeoff_tolerance                : " << takeoff_tolerance << " [m]" << TAIL << endl;
    cout << GREEN << "moving_around_tolerance          : " << moving_around_tolerance << " [m]" << TAIL << endl;
    cout << GREEN << "landing_tolerance                : " << landing_tolerance << " [m]" << TAIL << endl;

    cout << GREEN << "baudrate & portName              : " << baudrate << " Bd & " << portName << TAIL << endl;
    cout << GREEN << "cam_view_width                   : " << cam_view_width << " [m]" << TAIL << endl;
    cout << GREEN << "cam_view_height                  : " << cam_view_height << " [m]" << TAIL << endl;
    cout << GREEN << "tracking_tolerance               : " << tracking_tolerance << " [m]" << TAIL << endl;

    cout << std::defaultfloat;
    cout.unsetf(ios::fixed);
    cout.unsetf(ios::left);
    cout.unsetf(ios::showpoint);
    cout.unsetf(ios::showpos);

    if(moving_points.size() > 0)
    {
        cout << GREEN << "Moving around points             : ";
        for(int i = 0; i < moving_points.size(); i++)
        {
            auto point = moving_points.at(i);
            if(i == moving_points.size() - 1) cout << "[" << point[0] << " " << point[1] << "].";
            else                              cout << "[" << point[0] << " " << point[1] << "], ";
        }
        cout << TAIL <<endl;
    }
}

void topic_sub(ros::NodeHandle &nh)
{
    // 订阅遥控器状态
    static ros::Subscriber uav_control_state_sub = nh.subscribe<prometheus_msgs::UAVControlState>("/uav" + std::to_string(uav_id) + "/prometheus/control_state", 10, [&](const prometheus_msgs::UAVControlState::ConstPtr &msg) -> void
    { uav_control_state = *msg; });

    // 订阅无人机状态
    static ros::Subscriber uav_state_sub = nh.subscribe<prometheus_msgs::UAVState>("/uav" + to_string(uav_id) + "/prometheus/state", 10, [&](const prometheus_msgs::UAVState::ConstPtr &msg) -> void {
        uav_state = *msg; });

    // 订阅视觉检测状态
    static ros::Subscriber camera_detect_sub = nh.subscribe<prometheus_msgs::DetectionInfo>("/prometheus/object_detection/firepoint", 10, fire_point_cb);
}

static ros::Publisher uav_command_puber, uav_setup_puber;
void topic_adv(ros::NodeHandle &nh)
{
    uav_command_puber = nh.advertise<prometheus_msgs::UAVCommand>("/uav" + to_string(uav_id) + "/prometheus/command", 10);
    uav_setup_puber = nh.advertise<prometheus_msgs::UAVSetup>("/uav" + to_string(uav_id) + "/prometheus/setup", 10);
}

// 文件不存在，文件格式不正确，文件内没有航点信息，都返回false
bool init_flying_points(string &path, vector<Point> &points)
{
    ifstream ifs;
    ifs.open(path.c_str(), ios::in);
 
    if (!ifs.is_open())
    {
        cout << "read fail." << endl;
        return false;
    }

    string buf;
    bool succeed_flag = true;
    while (getline(ifs, buf))
    {
        string::size_type pos = buf.find(",");
        if(pos != string::npos)
        {
            string s1 = buf.substr(0, pos);
            string s2 = buf.substr(pos + 1, buf.size());
            float pos_x = stof(s1);
            float pos_y = stof(s2);
            points.emplace_back(pos_x, pos_y);
            // cout << pos_x << " " << pos_x << endl;
        }
        else
        {
            succeed_flag = false;
            break;
        }
    }
    return succeed_flag;
}

float limit_vel(float vel, float min_vel, float max_vel)
{
    float return_vel = 0.0;
    if(fabs(vel) < min_vel)
    {
        return_vel = vel / fabs(vel) * min_vel;
    }
    else if(fabs(vel) > max_vel)
    {
        return_vel = vel / fabs(vel) * max_vel;
    }
    else
    {
        return_vel = vel;
    }

    return return_vel;
}

int main(int argc, char** argv)
{
    ros::init(argc, argv, NODE_NAME);
    ros::NodeHandle nh("~");

    ros::Rate rate(30);

    read_param(nh);
    // 订阅话题
    topic_sub(nh);
    // 发布话题
    topic_adv(nh);

    vector<Point> moving_points;

    MySerial myser(baudrate, portName);

    bool res = init_flying_points(points_txt_path, moving_points);
    if(!res) 
    {
        ROS_ERROR_STREAM("Can not get moving points data from " << points_txt_path << " or can't open port.Program exiting!!");
        return 0;
    }
    print_param(moving_points);

    res = myser.open_serial();
    if(!res)
    {
        ROS_ERROR_STREAM("can't init open port: " << portName << ", " << baudrate << "Bd");
        // return 0;
    }
    

    Point home_pos;
    unsigned int current_point_index = 0;
    Point current_point = moving_points[current_point_index];
    bool low_height_to_1m = false, hover_3s = false, task_done = false;

    uav_command.Command_ID = 1;
    exec_state = EXEC_STATE::WaitingForStart;

    float abs_dis_x, abs_dis_y;
    float vel_x, vel_y;

    ros::Time t1, t_ser;

    ROS_WARN_STREAM("Please arm the vehicle and change control mode to COMMAND_CONTROL");

    t_ser = ros::Time::now();
    /*******  记录图像识别lost   ********/
    bool if_start_record_lost = false;
    ros::Time t_lost;
    while(ros::ok())
    {
        ros::spinOnce();

        if(ros::Time::now() - t_ser > ros::Duration(1.0) and exec_state != WaitingForStart and exec_state != Landing)
        {
            myser.write_pos(uav_state.position[0], uav_state.position[1], 1);
            t_ser = ros::Time::now();
        }

        switch(exec_state)
        {
            case WaitingForStart:
            {
                if (uav_control_state.control_state == prometheus_msgs::UAVControlState::COMMAND_CONTROL)
                {
                    ROS_INFO_STREAM("Entered COMMAND_CONTROL mode, progarm start running!!" );
                    home_pos[0] = uav_state.position[0];
                    home_pos[1] = uav_state.position[1];
                    exec_state = TakeOff;
                }  
                break; 
            }
            case TakeOff:
            {
                ROS_INFO_STREAM_ONCE("Enter takeoff mode!");
                if(fabs(uav_state.position[2] - target_height) < takeoff_tolerance)
                {
                    ROS_INFO_STREAM("UAV reached target height " << target_height << "[m], is going to moving around!");
                    exec_state = MovingAround;
                }
                else
                {
                    uav_command.Agent_CMD = prometheus_msgs::UAVCommand::Move;
                    uav_command.Move_mode = prometheus_msgs::UAVCommand::XYZ_POS;
                    uav_command.position_ref[0] = uav_state.position[0];
                    uav_command.position_ref[1] = uav_state.position[1];
                    uav_command.position_ref[2] = target_height;
                }
                break;
            }
            case MovingAround:
            {
                ROS_INFO_STREAM_ONCE("Enter moving around mode!");
                if(firepoint_det.is_detected && !task_done)
                {
                    exec_state = Tracking;
                    break;
                }
                
                abs_dis_x = fabs(uav_state.position[0] - current_point[0]);
                abs_dis_y = fabs(uav_state.position[1] - current_point[1]);
                if((abs_dis_x + abs_dis_y) < tracking_tolerance)
                {
                    ROS_INFO_STREAM("Uav arrived at the " << current_point_index << "-th point: [" << current_point[0] << ", " << current_point[1] << "] [m]");
                    if(current_point_index == moving_points.size() - 1)
                    {
                        ROS_INFO_STREAM("Arrived at the last point, trying to return!");
                        exec_state = Return;
                    }
                    else
                    {
                        current_point_index ++;
                        current_point = moving_points[current_point_index];
                        ROS_INFO_STREAM("Moving to Next point: [" << current_point[0] << ", " << current_point[1] << "] [m]");
                    }
                }
                else
                {
                    vel_x = (current_point[0] - uav_state.position[0]) * 0.3;
                    vel_y = (current_point[1] - uav_state.position[1]) * 0.3;

                    vel_x = limit_vel(vel_x, min_vel_moving_around, max_vel_moving_around);
                    vel_y = limit_vel(vel_y, min_vel_moving_around, max_vel_moving_around);
                    uav_command.Agent_CMD = prometheus_msgs::UAVCommand::Move;
                    uav_command.Move_mode = prometheus_msgs::UAVCommand::XY_VEL_Z_POS;
                    uav_command.velocity_ref[0] = vel_x;
                    uav_command.velocity_ref[1] = vel_y;
                    uav_command.position_ref[2] = target_height;
                }
                break;
            }
            
            case Tracking:
            {
                ROS_INFO_STREAM_ONCE("Detect fire target, tracing!");
                
                if(!firepoint_det.is_detected)
                {
                    exec_state = Lost;
                    break;
                }
                float abs_x = fabs(firepoint_det.pos_body_frame[0] * cam_view_height * uav_state.position[2] / 2);
                float abs_y = fabs(firepoint_det.pos_body_frame[1] * cam_view_width * uav_state.position[2] / 2);
                ROS_INFO_STREAM_THROTTLE(2, "Distance to firepoint is: " << abs_x + abs_y << " [m]");
                if(abs_x < tracking_tolerance and abs_y < tracking_tolerance)
                    {
                        ROS_INFO_STREAM( "Arrived at firepoint, start to execute task!");
                        exec_state = ExecTask;
                        break;
                    }
                else
                {
                    vel_x = limit_vel(firepoint_det.pos_body_frame[0], min_vel_tracking, max_vel_tracking);
                    vel_y = limit_vel(firepoint_det.pos_body_frame[1], min_vel_tracking, max_vel_tracking);
                    uav_command.Agent_CMD = prometheus_msgs::UAVCommand::Move;
                    uav_command.Move_mode = prometheus_msgs::UAVCommand::XY_VEL_Z_POS;
                    uav_command.velocity_ref[0] = vel_x;
                    uav_command.velocity_ref[1] = vel_y;
                    uav_command.position_ref[2] = target_height;
                }
                break;
            }

            case Lost:
            {
                ROS_INFO_STREAM_ONCE("Enter lost mode!");
                if(!if_start_record_lost)
                {
                    if_start_record_lost = true;
                    t_lost = ros::Time::now();
                    ROS_INFO_STREAM("uav lost target, will hover for 10s, if can't regain target, Uav will return!");
                }
                else
                {
                    if(ros::Time::now() - t_lost > ros::Duration(10.0))
                    {
                        ROS_INFO_STREAM("can't regain target within 10s, mission fail! Return to home pos!");
                        exec_state = Return;
                        break;
                    }
                }
                if(firepoint_det.is_detected)
                {
                    ROS_INFO_STREAM("regian target! Back to Tracing mode!");
                    exec_state = Tracking;
                    break;
                }
                uav_command.Agent_CMD = prometheus_msgs::UAVCommand::Current_Pos_Hover;
                break;
            }

            case ExecTask:
            {
                ROS_INFO_STREAM_ONCE("Executing Task, Uav will lower to 1.0 [m]!");
                if(!low_height_to_1m)
                {
                    if(fabs(uav_state.position[2] - 1.0) < 0.1)
                    {
                        low_height_to_1m = true;
                        t1 = ros::Time::now();
                        break;
                    }
                    uav_command.Agent_CMD = prometheus_msgs::UAVCommand::Move;
                    uav_command.Move_mode = prometheus_msgs::UAVCommand::XYZ_POS;
                    uav_command.position_ref[0] = uav_state.position[0];
                    uav_command.position_ref[1] = uav_state.position[1];
                    uav_command.position_ref[2] = 1.0;
                    break;
                }

                ROS_INFO_STREAM_ONCE("Executing Task, Uav will Hover for 4s");
                if(!hover_3s)
                {
                    if((ros::Time::now() - t1) > ros::Duration(4.0))
                    {
                        hover_3s = true;
                        break;
                    }
                    uav_command.Agent_CMD = prometheus_msgs::UAVCommand::Current_Pos_Hover;
                    break;
                }
                // TODO 投放灭火包
                // TODO 火源地点发送给消防车
                myser.write_pos(uav_state.position[0], uav_state.position[1], 2);
                
                ROS_INFO_STREAM("Finished work, back to moving around!");
                exec_state = MovingAround;
                task_done = true;
                break;
            }

            case Return:
            {
                ROS_INFO_STREAM_ONCE("At Return mode, moving to home pos: " << home_pos[0] << " " << home_pos[1]);
                abs_dis_x = fabs(uav_state.position[0] - home_pos[0]);
                abs_dis_y = fabs(uav_state.position[1] - home_pos[1]);
                if(abs_dis_x < landing_tolerance && abs_dis_y < landing_tolerance)
                {
                    ROS_INFO_STREAM("Uav arrived at the Land Point, Landing!");
                    exec_state = Landing;
                }
                else
                {
                    vel_x = (home_pos[0] - uav_state.position[0]) * 0.3;
                    vel_y = (home_pos[1] - uav_state.position[1]) * 0.3;

                    vel_x = limit_vel(vel_x, min_vel_moving_around, max_vel_moving_around);
                    vel_y = limit_vel(vel_y, min_vel_moving_around, max_vel_moving_around);
                    uav_command.Agent_CMD = prometheus_msgs::UAVCommand::Move;
                    uav_command.Move_mode = prometheus_msgs::UAVCommand::XY_VEL_Z_POS;
                    uav_command.velocity_ref[0] = vel_x;
                    uav_command.velocity_ref[1] = vel_y;
                    uav_command.position_ref[2] = target_height;
                }
                break;
            }
            case Landing:
            {
                ROS_INFO_STREAM_ONCE("Enter landing mode!");
                uav_command.Agent_CMD = prometheus_msgs::UAVCommand::Land;
                if(ros::Time::now() - t1 > ros::Duration(0.5))
                {
                    myser.write_pos(uav_state.position[0], uav_state.position[1], 3);
                    t1 = ros::Time::now();
                }
                break;
            }
        }

        if(exec_state == Landing && !uav_state.armed)
        {
            ROS_INFO_STREAM("Uav Landed and Disarmed! Program end!");
            break;
        }

        uav_command.header.stamp = ros::Time::now();
        uav_command.Command_ID = uav_command.Command_ID + 1;
        uav_command_puber.publish(uav_command);

        rate.sleep();
    }

    return 0;
}

